#include "planner.h"

void PlannerInit();
void PlaneXY(float X, float Y, float speed);

float FeedSpeed = 0;
float TravelSpeed = 3000 / 60;

// QueueHandle_t PlannerQueueHandle = NULL;

QueueHandle_t XPlannerDAValQueueHandle = NULL;
QueueHandle_t YPlannerDAValQueueHandle = NULL;
QueueHandle_t TimesInterpolationQueueHandle = NULL;

void PlannerInit()
{
    // PlannerQueueHandle = xQueueCreate(5, sizeof(struct CommandBlock));

    XPlannerDAValQueueHandle = xQueueCreate(5, sizeof(uint16_t *));
    YPlannerDAValQueueHandle = xQueueCreate(5, sizeof(uint16_t *));
    TimesInterpolationQueueHandle = xQueueCreate(5, sizeof(int *));
}

float RadValX, RadValY;
float kx = 3.5, ky = 1.2;
static uint16_t DisToDAX(float x, float y)
{
    RadValX = atan(x / (sqrt(DisFTheta * DisFTheta + y * y) + DisXYMirror));
    RadValY = atan(y / DisFTheta);
    return (atan((x - (DisFTheta * tan(RadValX) * (1 / cos(RadValY) - 1))) / (sqrt(DisFTheta * DisFTheta + y * y) + DisXYMirror)) / RAD_MG_MAX * 32768 + 32767 - 1430) * 5 / BaseVol;
}

static uint16_t DisToDAY(float y)
{
    return (RadValY / RAD_MG_MAX * 32768 + 32767 - 1440) * 5 / BaseVol;
}

float GoalX, GoalY;
float DistX, DistY;
float StartX, StartY;
float PlaneX, PlaneY;
float StepX, StepY;
int TimesLinearInterpolation[DAValBufSize];
int InterpolationTimesBufCount;
int InterpolationValCount;

uint16_t DAValBufX[DAValBufSize][DAValArraySize];
uint16_t DAValBufY[DAValBufSize][DAValArraySize];

void PlaneXY(float X, float Y, float speed /* , struct CommandBlock *cb */)
{
    uint16_t *pDAValBufX = DAValBufX[InterpolationTimesBufCount];
    uint16_t *pDAValBufY = DAValBufY[InterpolationTimesBufCount];
    int *pTimes = &TimesLinearInterpolation[InterpolationTimesBufCount];

    GoalX = X;
    GoalY = Y;

    DistX = GoalX - StartX;
    DistY = GoalY - StartY;

    TimesLinearInterpolation[InterpolationTimesBufCount] = sqrt(DistX * DistX + DistY * DistY) / (float)speed * (float)MotionsPerSecond;
    // long distance, compute Interpolation
    if (TimesLinearInterpolation[InterpolationTimesBufCount])
    {
        if (DistX > 0.00001 || DistX < -0.00001)
        {
            StepX = DistX / TimesLinearInterpolation[InterpolationTimesBufCount];
        }
        else
        {
            StepX = 0;
        }

        if (DistY > 0.00001 || DistY < -0.00001)
        {
            StepY = DistY / TimesLinearInterpolation[InterpolationTimesBufCount];
        }
        else
        {
            StepY = 0;
        }
        PlaneX = StartX;
        PlaneY = StartY;
        while (InterpolationValCount < TimesLinearInterpolation[InterpolationTimesBufCount])
        {
            PlaneX += StepX;
            PlaneY += StepY;

            DAValBufX[InterpolationTimesBufCount][InterpolationValCount] = DisToDAX(PlaneX, PlaneY);
            DAValBufY[InterpolationTimesBufCount][InterpolationValCount] = DisToDAY(PlaneY);

            InterpolationValCount++;
        }
        InterpolationValCount = 0;
    }
    else
    {
        TimesLinearInterpolation[InterpolationTimesBufCount] = 1;

        DAValBufX[InterpolationTimesBufCount][0] = DisToDAX(GoalX, GoalY);
        DAValBufY[InterpolationTimesBufCount][0] = DisToDAY(GoalY);
    }
    StartX = GoalX;
    StartY = GoalY;

    InterpolationTimesBufCount = (InterpolationTimesBufCount + 1) % DAValBufSize;
    xQueueSendToBack(XPlannerDAValQueueHandle, &pDAValBufX, portMAX_DELAY);
    xQueueSendToBack(YPlannerDAValQueueHandle, &pDAValBufY, portMAX_DELAY);
    xQueueSendToBack(TimesInterpolationQueueHandle, &pTimes, portMAX_DELAY);
}
